/* -*- mode: C++; c-file-style: "stroustrup"; c-basic-offset: 4; indent-tabs-mode: nil; -*- */

// Compile like this: g++ -O2 -Wall matrix.cpp -o matrix -g -std=gnu++0x

#include <iostream>
#include <cstdlib>
#include <cassert>
#include <cstring>
#include <limits.h>
#include <math.h>
#include <vector>
#include <memory>

/* For testing, you can use a
 * constant value if you wish.
 */
#define RANGE(S) (S)

// Precision for checking identity.
#define PRECISION 1e-5

using namespace std;

/*
 * Random generation, but always the same for
 * a given size for fair comparison.
 */
template <typename T> 
unique_ptr<T[]> gen_mat(const size_t dim)
{
    srand48(dim);
	unique_ptr<T[]> a (new T[dim*dim]());
    for(size_t i = 0; i < dim; ++i)
    {
        for(size_t j = 0; j < dim; ++j)
        {
            T z = (drand48() - 0.5)*RANGE(dim);
            a[i*dim+j] = (z < 10*PRECISION && z > -10*PRECISION) ? 0.0 : z;
        }
    }
	return a;
}

/* 
 * Creates an identity matrix
 */
template <typename T> 
unique_ptr<T[]> gen_mat_identity(const size_t dim)
{
    srand48(dim);
	unique_ptr<T[]> a (new T[dim*dim]());
    for(size_t i = 0; i < dim; ++i)
    {
        for(size_t j = 0; j < dim; ++j)
        {
			(i == j) ? a[i*dim+j] = 1.0 : a[i*dim+j] = 0.0;
        }
    }
	return a;
}

/* 
 * custom exception from Session 4
 */
class MatrixException: public exception
{
public:
	virtual const char* what() const throw()
	{
		return "Unmatched Dimensions";
	}
};

/* 
 * Check that a divider is not too close to 0. 
 */
template <typename T> 
void check_divider(T x)
{
    if (fabs(x) < PRECISION)
    {
        cerr << "Matrix non-invertible or computationally unstable.\n";
        abort();
    }
}

/* ORIGINAL */
/* Read a re-arranged matrix. */
#define A(I,J) a[index[I]*n+(J)]
#define B(I,J) b[index[I]*n+(J)]

/* a = invert(input), using b as temporary matrix
 * Do not mind the signature, this is old code.
 * You do not need to look at the implementation in detail.
 */
 /*
  * The typename T have to be of floating point type. Any type that can pass that check can use the inv_mat function.
  * if it is called with a int the program will not compile.
 */
template <typename T> 
void inv_mat(const T *input, T *a, T *b, size_t n, typename std::enable_if<std::is_floating_point<T>::value >::type* = 0)
{
    size_t index[n];

    // Init a, b, and index.
    memcpy(a, input, n*n*sizeof(T));
    memset(b, 0, n*n*sizeof(T));
    for(size_t i = 0; i < n; ++i)
    {
        index[i] = i;   // Identity mapping.
        b[i*n+i] = 1.0; // Identity matrix.
    }

    // Gaussian elimination.
    for(size_t k = 0; k < n; ++k)
    {
        // Find pivot.
        T x = fabs(A(k,k));
        size_t j = k;
        for(size_t i = k+1; i < n; ++i)
        {
            T y = fabs(A(i,k));
            if (y > x)
            {
                j = i;
                x = y;
            }
        }

        // Swap j <-> k.
        size_t i = index[j];
        index[j] = index[k];
        index[k] = i;

        // Division step.
        x = A(k,k);
        A(k,k) = 1.0;
        check_divider(x);
        for(size_t j = k+1; j < n; ++j) A(k,j) /= x;
        for(size_t j = 0  ; j < n; ++j) B(k,j) /= x;

        // Elimination step.
        for(i = k+1; i < n; ++i)
        {
            for(j = k+1; j < n; ++j) A(i,j) -= A(i,k)*A(k,j);
            for(j = 0  ; j < n; ++j) B(i,j) -= A(i,k)*B(k,j);
            A(i,k) = 0.0;
        }
    }

    // Back-substitution.
    for(size_t k = n-1; k > 0; --k)
    {
        for(size_t i = k-1; (int)i >= 0; --i)
        {
            for(size_t j = 0; j < n; ++j) B(i,j) -= A(i,k)*B(k,j);
            //A(i,k) = 0.0; -- implicit and not used later.
        }
    }

    // Result.
    for(size_t i = 0; i < n; ++i)
    {
        for(size_t j = 0; j < n; ++j)
        {
            a[i*n+j] = B(i,j);
        }
    }
}


template <typename T> 
bool check_identity(const T *m, size_t dim)
{
    for(size_t i = 0; i < dim; ++i)
    {
        for(size_t j = 0; j < dim; ++j)
        {
            T r = i == j ? m[i*dim+j] - 1.0 : m[i*dim+j];
            if (r < -PRECISION || r > PRECISION)
            {
                return false;
            }
        }
    }
	return true;
}

/*
 * It is possible to swap this implementation
*/
template <typename T> 
class MatrixImplementation 
{
protected:
	size_t dim;
    unique_ptr<T[]> m; //To avoid memory leak

	//Original addition function moved to protected scope
	template <typename I> 
	void mat_add(const I *a, const I *b, I *c, size_t dim)
	{
		assert(a != c && b != c);
		for(size_t i = 0; i < dim*dim; ++i)
		{
			c[i] = a[i] + b[i];
		}
	}
	
	//Original multiply function moved to protected scope
	template <typename U>  //c = result  a = input1  b = input2
	void mat_mult(const U *a, const U *b, U *c, size_t dim)
	{
		
		assert(a != c && b != c);

		/* Re-arranged matrix multiplication - variant. */
		for(size_t i = 0; i < dim; ++i)
		{
			for(size_t j = 0; j < dim; ++j)
			{
				c[i*dim+j] = 0;
			}
			for(size_t k = 0; k < dim; ++k)
			{
				for(size_t j = 0; j < dim; ++j)
				{
					c[i*dim+j] += a[i*dim+k]*b[k*dim+j];
				}
			}
		}    
	}
public:
	//This need to be explicit because a function in my test case would call it implicit
	explicit MatrixImplementation(size_t dim, bool identity = false) : dim(dim), m(new T[dim*dim]())
    {
		//This makes it easy for the user to get an identity matrix
        m = identity ? gen_mat_identity<T>(dim) : gen_mat<T>(dim);
    }
    MatrixImplementation(T* m, size_t dim):  dim(dim), m(m){} 
    ~MatrixImplementation(){  
    };
	
	MatrixImplementation( const MatrixImplementation& other ) : //copy constructor
     dim( other.dim ), m(new T[other.dim*other.dim]())
	{
		for(size_t i = 0; i < dim*dim; ++i)
		{
			m[i] = other.m[i];
		}
	}

	MatrixImplementation<T>& operator=(const MatrixImplementation& rhs)
	{
		if (this == &rhs)      // Same object?
		{return *this;}
		//The existing m is deleted and a new takes its place
		m.reset(new T[rhs.dim*rhs.dim]());
		//A new real copy is made for the dim, otherwise it would be deleted.
		memcpy ( m.get(), rhs.m.get(), rhs.dim*rhs.dim*sizeof(T));
		this->dim = rhs.dim;
		return *this;
	}
	
	bool operator==(const MatrixImplementation<T>& rhs)
	{	
		//if dim do not fit then they are not equal.
		if(this->dim != rhs.dim)
		{
			return false;
		}
		for(size_t i = 0; i < dim*dim; ++i)
		{
			if(this->m[i] != rhs.m[i])
			{
				return false;
			}
		}
		return true;
	}
	
	MatrixImplementation<T> & operator+=(const MatrixImplementation<T>& rhs)
	{	
		//exception is thrown if dim are not equal. 
		if(this->dim != rhs.dim)
		{
			throw MatrixException();
		}
		MatrixImplementation<T> result(dim, false);
		mat_add(rhs.m.get(),this->m.get(),result.m.get(),dim);
		swap(result);
		return *this;
	}
	
	
    const MatrixImplementation<T> operator+(const MatrixImplementation<T>& rhs)
	{	
		//Calles the += operator
		MatrixImplementation<T> result(dim, false);    
		result	= *this;
		result += rhs; 
		return result;
	}

	MatrixImplementation<T> & operator*=(const MatrixImplementation<T>& rhs)
	{
		if(this->dim != rhs.dim)
		{
			throw MatrixException();
		}
		MatrixImplementation<T> result(dim, false);
		mat_mult(rhs.m.get(), this->m.get(), result.m.get(), dim);
		swap(result);
		return *this;
	}
	
    const MatrixImplementation<T> operator*(const MatrixImplementation<T>& rhs)
	{
		MatrixImplementation<T> result(*this); //Use copy constructor   
		result *= rhs;  
		return result;
	}
	
	template <typename O> 
	const MatrixImplementation<T> operator*(const O rhs)
	{
		MatrixImplementation<T> result(dim, false);
		for(size_t i = 0; i < dim*dim; ++i)
		{
			result.m[i] = this->m[i]*rhs; 
		}
		return result;
	}

    bool isIdentity()
	{
		return check_identity<T>(this->m.get(),dim);
	}

    void invert()
	{
		MatrixImplementation<T> result(dim, false);
		MatrixImplementation<T> temp(dim, false);
		inv_mat(this->m.get(), result.m.get(), temp.m.get(), this->dim);
		this->m = std::move(result.m);
	}

	void swap(MatrixImplementation& rhs)
	{
		std::swap(this->m,rhs.m);
		std::swap(this->dim,rhs.dim);
	}
	
	template <typename Y> 
    friend std::ostream& operator<<(std::ostream& stream, MatrixImplementation<Y>& mat);
	template <typename D> friend class Matrix;
}; 

//since I am using c++ then it would be odd to implement my own reference counting. 
//Depending on what the libary is used for then different implementation for reference counting would make sense.
//By using shared_ptr I limit my own freedom. In this case it dosent make any difference.
template <typename A> 
class Matrix 
{
private:
	shared_ptr<MatrixImplementation<A>> innerMatrix;
	Matrix(MatrixImplementation<A> inner): innerMatrix(new MatrixImplementation<A>(inner)) {
	
	}
public:
	explicit Matrix(size_t dim, bool identity = false) : innerMatrix(new MatrixImplementation<A>(dim,identity))
    {};
	
    Matrix( const Matrix& other ) : innerMatrix(other.innerMatrix){};//copy constructor
	
	
	~Matrix(){};

	size_t getDim()
	{
		return innerMatrix->dim;
	}
	
	A * getM()
	{
		return innerMatrix->m.get();
	}
	
	//From here on down the funcitons just utilize the implementation in the matrixImplementation.
	template <typename D> 
	const Matrix<A> operator*(const D rhs)
	{
		return Matrix<A>(*this->innerMatrix.get() * rhs);
	}
	
	const Matrix<A> operator*(const Matrix<A>& rhs)
	{
		return Matrix<A>(*this->innerMatrix.get() * *rhs.innerMatrix.get());
	}
	
	Matrix<A> & operator*=(const Matrix<A>& rhs)
	{
		 *this->innerMatrix.get() *= *rhs.innerMatrix.get();
		 return *this;
	}
	
	Matrix<A>& operator=(const Matrix& rhs)
	{
		*this->innerMatrix.get() = *rhs.innerMatrix.get();
		return *this;
	}
	
	bool operator==(const Matrix<A>& rhs)
	{
		return *this->innerMatrix.get() == *rhs.innerMatrix.get();
	}
	
	Matrix<A> & operator+=(const Matrix<A>& rhs)
	{ 
		this->innerMatrix += rhs.innerMatrix;
		return *this;
	}
    
	const Matrix<A> operator+(const Matrix<A>& rhs)
	{
		return Matrix<A>(*this->innerMatrix.get() + *rhs.innerMatrix.get());
	}
	
	void swap(Matrix& rhs)
	{
		this->innerMatrix->swap(rhs.innerMatrix);
	}
	
	void invert()
	{
		this->innerMatrix->invert();
	}
	
	bool isIdentity()
	{
		return this->innerMatrix->isIdentity();
	}
}; 

//the implementation method makes this friend.
template <typename S> 
std::ostream& operator<<(std::ostream& stream, Matrix<S>& mat)
{
    for(size_t i = 0; mat.getDim() * mat.getDim() > i; ++i)
    {
		(i % mat.getDim() == 0) ? stream << "[":false;  
		stream << mat.getM()[i] << ",";
        (i % mat.getDim() == mat.getDim()-1) ? stream << "]" << "\n":false; 
    }   
    stream << "\n";
    return stream;
}

void test(const size_t dim)
{
	//identity gen
	cout << "identity gen: ";
	Matrix<double> h(dim, true);
	cout << "\t\t" << ((h.isIdentity())?"true":"false") << endl;

	
	//assignment 
	cout << "assignment: ";
	Matrix<double> g(dim, false);
	g = h;
	cout << "\t\t" << ((g.isIdentity())?"true":"false") << endl;

	//a*d^-1 = identity //multiply
	cout << "a*a^-1 = identity: ";
	Matrix<double> result(dim, false);
	Matrix<double> temp(dim, false);
	Matrix<double> input(dim, false);
	result = input;
	input.invert();
		
	//inv_mat(input.m,result.m,temp.m,dim);
	//cout << endl << temp << endl ;
	temp = input*result;
	//cout << endl << temp << endl ;
	cout << "\t" << ((temp.isIdentity())?"true":"false") << endl;

	//Comparison
	cout << "Compare: ";
	Matrix<double> i(dim, false);
	Matrix<double> j(dim, false);
	j = i;
	cout << "\t\t" <<((i == j)?"true":"false") << endl;
	
	//plus
	Matrix<double> e(dim, false);
	Matrix<double> plusResult(dim, false);
	Matrix<double> multiResult(dim, false);
	cout << "plus: ";
	plusResult = e + e;
	multiResult = e * 2;
	cout << "\t\t\t" << ((plusResult == multiResult)?"true":"false") << endl;

	
	//float
	cout << "float: ";
	
	Matrix<float> floatresult(dim, false);
	Matrix<float> floattemp(dim, false);
	Matrix<float> floatinput(dim, false);
	floatresult = floatinput;
	floatinput.invert();
	floattemp = floatinput*floatresult;
	cout << "\t\t\t" << ((floattemp.isIdentity())?"true":"false") << endl;
		
	//long
	cout << "long: ";
	Matrix<long> intresult(dim, false);
	Matrix<long> inttemp(dim, false);
	Matrix<long> intinput(dim, false);
	intresult = intinput * 2;
	inttemp = intinput + intinput;
	cout << "\t\t\t" << ((inttemp == intresult)?"true":"false") << endl;
	
	//exception
	cout << "exception: ";
	Matrix<float> excep1(2, false);
	Matrix<float> excep2(4, false);
	try
	{
		(excep1 * excep2);
	}
	catch(MatrixException& e)
	{
		cout << "\t\t" << "true" << endl;
		
	}
	
}
int main(int argc, char *argv[])
{
    if (argc < 2)
    {
        cerr << "Usage: " << argv[0] << " size\n";
        return 1;
    }
    else
    {
        int i = atoi(argv[1]);
        if (i < 1)
        {
            cerr << "Minimum dimension is 1.\n";
            return 1;
        }
        test(i);
        return 0;
    }
}
